#include <stdio.h>
#include <stdlib.h>
#include <linux/i2c-dev.h>
#include <fcntl.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>

// VTMotor firmware on Grove I2C Motor Driver's command define.
#define DRV_WHO_AM_I	0x10
#define DRV_DRV_ENABLE	0x11
#define DRV_DRV_DISABLE	0x12
#define DRV_SET_SPEED	0x13
#define DRV_SET_DIRECTION	0x14

// Driver's I2C address.
static unsigned char MOTOR_DRIVER_ADDRESS = 0x28;

// I2C filename.
char *i2cFileName = "/dev/i2c-1";
//char *i2cFileName = "/dev/i2c-0";	// first lot.

// Prototype of function.
int SetMotorSpeed(int leftSpeed, int rightSpeed);

int main()
{
	printf("***** VTMotor firmware on Grove I2C Driver test *****");

	SetMotorSpeed(0, 0);
	sleep(1);
	SetMotorSpeed(40, 40);
	sleep(3);
	SetMotorSpeed(80, 80);
	sleep(3);
	SetMotorSpeed(40, -40);
	sleep(3);
	SetMotorSpeed(-40, 40);
	sleep(3);
	SetMotorSpeed(0, 0);

	return 0;
}

int SetMotorSpeed(int leftSpeed, int rightSpeed)
{
	int fd;		// file descriptor.
	unsigned char buf[10];
	unsigned char lSpd, rSpd;

	int address = (int)MOTOR_DRIVER_ADDRESS;	// device address.

	// open I2C port as read/write.
	if ((fd = open(i2cFileName, O_RDWR) < 0))
	{
		printf("Failed to open I2C port\n");
		return (-1);
	}

	// port option and device address setting.
	if (ioctl(fd, I2C_SLAVE, address) < 0)
	{
		printf("Unable to get bud access to talk to slave\n");
		close(fd);
		return (-2);
	}

	// send turn on.
	buf[0] = DRV_DRV_ENABLE;
	if (write(fd, buf, 1) != 1)
	{
		printf("Error sendding to turn on.");
		close(fd);
		return (-3);
	}

	// send direction.
	buf[0] = DRV_SET_DIRECTION;
	if (leftSpeed < 0 )
	{
		buf[1] = 0x1;
		lSpd = (unsigned char)(-1 * leftSpeed);
	}
	else
	{
		buf[1] = 0x0;
		lSpd = (unsigned char)leftSpeed;
	}
	if (rightSpeed < 0)
	{
		buf[2] = 0x1;
		rSpd = (unsigned char)(-1 * rightSpeed);
	}
	else
	{
		buf[2] = 0x0;
		rSpd = (unsigned char)rightSpeed;
	}
	if (write(fd, buf, 3) != 3)
	{
		printf("Eroor sendding to direction.");
		close(fd);
		return (-4);
	}

	// send speed.
	buf[0] = DRV_SET_SPEED;
	buf[1] = lSpd;
	buf[2] = rSpd;
	if (write(fd, buf, 3) != 3)
	{
		printf("Eroor sendding to direction.");
		close(fd);
		return (-5);
	}

	close(fd);

	return (0);
}
	